#include "OptoCounter.h"
#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <sstream>
#include <cstdio>
#include <fcntl.h>
#include <poll.h>

OptoCounter::OptoCounter(int gpioNumber, double freqDiv):
m_GpioNumber(gpioNumber),
m_FreqDiv(freqDiv),
m_CountingThread(NULL),
m_shallStop(true),
m_NbToursParSeconds(0)
{
	std::stringstream gpioNumberStr;
	gpioNumberStr << gpioNumber;

	int ExportFd = open("/sys/class/gpio/export", O_WRONLY);
	write(ExportFd, gpioNumberStr.str().c_str(), gpioNumberStr.str().size());
	close(ExportFd);

	std::stringstream DirectionFileName;
	DirectionFileName << "/sys/class/gpio/gpio" << gpioNumber << "/direction";
	
	int DirectionFd = open(DirectionFileName.str().c_str(), O_WRONLY);
	write(DirectionFd, "in", 2);
	close(DirectionFd);

	std::stringstream EdgeFileName;
	EdgeFileName<< "/sys/class/gpio/gpio" << gpioNumber << "/edge";
	
	int EdgeFd = open(EdgeFileName.str().c_str(), O_WRONLY);
	write(EdgeFd , "rising", 6);
	close(EdgeFd );

	std::stringstream FileName;
	FileName << "/sys/class/gpio/gpio" << m_GpioNumber << "/value";
	
	m_GpioFd= open(FileName.str().c_str(),O_RDONLY);
}

OptoCounter::~OptoCounter()
{
	close(m_GpioFd);
	this->Stop();
}

void OptoCounter::CountingThread()
{
	
	struct pollfd pfd;

	pfd.fd = m_GpioFd;
	pfd.events = POLLPRI;
	pfd.revents = 0;
	
	char c;

	struct timeval lastTime, currentTime;
	gettimeofday(&lastTime, NULL);
	
	m_NbToursParSeconds = 0;

	while(false == m_shallStop)
	{
		int pollReturn = poll(&pfd, 1, 1000);
		
		if(pollReturn < 1)
		{
			m_NbToursParSeconds = 0;
			//std::cout << "Pool return -1" << std::endl;
			continue;
		}

		read(m_GpioFd, &c, 1);
		gettimeofday(&currentTime, NULL);

		double DeltatTime = (currentTime.tv_sec + currentTime.tv_usec * 0.000001) - 
			      		(lastTime.tv_sec + lastTime.tv_usec * 0.000001);
			
		m_NbToursParSeconds = (1. / DeltatTime) / m_FreqDiv;
		
		lastTime = currentTime;
	}	
}

void OptoCounter::Run()
{
	m_shallStop = false;
	boost::function<void (void)>Count = boost::bind(&OptoCounter::CountingThread, this);
	m_CountingThread = new boost::thread(Count);

	pthread_t threadID = (pthread_t) m_CountingThread->native_handle();

	struct sched_param param;
	int policy;

	pthread_getschedparam(threadID, &policy, &param);
	policy = SCHED_FIFO;
	param.sched_priority = 4;

	pthread_setschedparam(threadID, policy, &param);
}

void OptoCounter::Stop()
{
	m_shallStop = true;
	if(m_CountingThread != NULL)
	{
		m_CountingThread->join();
		delete m_CountingThread;
	}
	std::cout << "OPTO Stopped!" << std::endl;
}

